﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Util.Controls;
using VisionConfig;

namespace FalionForm.ComAdd
{
    /// <summary>
    /// ComAdd_2.xaml 的交互逻辑
    /// </summary>
    public partial class ComAdd_2 : Page
    {
        public ComAdd_2()
        {
            InitializeComponent();
            MainWindow.SClient.OnmessageEvent += RecMsg;
        }
        Thread GetPose;
        FButton FB;

        //多线程
        void GenPose()
        {
            HTuple Model3D, pose;
            try
            {               
                MainPage.pro.Calib(out Model3D,out pose);
                Page3._dp.DramModel = true;
                MainPage.pro.StartDisplay(Page3._dp.HW);
            }
            catch
            {
                pose = new HTuple();
                this.Dispatcher.Invoke(new Action(() =>
                {
                    FB.IsEnabled = true;
                }));
            }
            HTuple Pose = new HTuple();
            Pose = pose.Clone();
          
            this.Dispatcher.Invoke(new Action(() => 
            {
                FB.IsEnabled = true;
                if (Pose.Length == 0)
                {
                    return;
                }               
                switch (FB.Content.ToString().Trim())
                {
                    #region
                    case "1":
                        PublicData.calibDataSave.Picture_x1 = Pose[0].D;
                        PublicData.calibDataSave.Picture_y1 = Pose[1].D;
                        PublicData.calibDataSave.Picture_z1 = Pose[2].D;
                        PublicData.calibDataSave.Picture_g1 = Pose[3].D;
                        PublicData.calibDataSave.Picture_b1 = Pose[4].D;
                        PublicData.calibDataSave.Picture_a1 = Pose[5].D;
                        break;
                    case "2":
                        PublicData.calibDataSave.Picture_x2 = Pose[0].D;
                        PublicData.calibDataSave.Picture_y2 = Pose[1].D;
                        PublicData.calibDataSave.Picture_z2 = Pose[2].D;
                        PublicData.calibDataSave.Picture_g2 = Pose[3].D;
                        PublicData.calibDataSave.Picture_b2 = Pose[4].D;
                        PublicData.calibDataSave.Picture_a2 = Pose[5].D;
                        break;
                    case "3":
                        PublicData.calibDataSave.Picture_x3 = Pose[0].D;
                        PublicData.calibDataSave.Picture_y3 = Pose[1].D;
                        PublicData.calibDataSave.Picture_z3 = Pose[2].D;
                        PublicData.calibDataSave.Picture_g3 = Pose[3].D;
                        PublicData.calibDataSave.Picture_b3 = Pose[4].D;
                        PublicData.calibDataSave.Picture_a3 = Pose[5].D;
                        break;
                    case "4":
                        PublicData.calibDataSave.Picture_x4 = Pose[0].D;
                        PublicData.calibDataSave.Picture_y4 = Pose[1].D;
                        PublicData.calibDataSave.Picture_z4 = Pose[2].D;
                        PublicData.calibDataSave.Picture_g4 = Pose[3].D;
                        PublicData.calibDataSave.Picture_b4 = Pose[4].D;
                        PublicData.calibDataSave.Picture_a4 = Pose[5].D;
                        break;
                    case "5":
                        PublicData.calibDataSave.Picture_x5 = Pose[0].D;
                        PublicData.calibDataSave.Picture_y5 = Pose[1].D;
                        PublicData.calibDataSave.Picture_z5 = Pose[2].D;
                        PublicData.calibDataSave.Picture_g5 = Pose[3].D;
                        PublicData.calibDataSave.Picture_b5 = Pose[4].D;
                        PublicData.calibDataSave.Picture_a5 = Pose[5].D;
                        break;
                    case "6":
                        PublicData.calibDataSave.Picture_x6 = Pose[0].D;
                        PublicData.calibDataSave.Picture_y6 = Pose[1].D;
                        PublicData.calibDataSave.Picture_z6 = Pose[2].D;
                        PublicData.calibDataSave.Picture_g6 = Pose[3].D;
                        PublicData.calibDataSave.Picture_b6 = Pose[4].D;
                        PublicData.calibDataSave.Picture_a6 = Pose[5].D;
                        break;
                    default:
                        break;
                        #endregion
                }             
            }));
        }
        //相机坐标获取
        private void btn_camera(object sender, RoutedEventArgs e)
        {
            FB = (FButton)sender;
            FB.IsEnabled = false;

            GetPose = new System.Threading.Thread(new ThreadStart(GenPose));
            GetPose.IsBackground = true;
            GetPose.Start();
        }

        //机器人坐标获取
        private void btn_robot(object sender, RoutedEventArgs e)
        {
            HTuple Pose;
            if (!MainPage.pro.GetRobotPose(out Pose))
            {
                return;
            }


            FButton FB = (FButton)sender;
            switch (FB.Content.ToString().Trim())
            {
                #region
                case "1":
                    PublicData.calibDataSave.Robot_x1 = Pose[0].D;
                    PublicData.calibDataSave.Robot_y1 = Pose[1].D;
                    PublicData.calibDataSave.Robot_z1 = Pose[2].D;
                    PublicData.calibDataSave.Robot_g1 = Pose[3].D;
                    PublicData.calibDataSave.Robot_b1 = Pose[4].D;
                    PublicData.calibDataSave.Robot_a1 = Pose[5].D;
                    break;
                case "2":
                    PublicData.calibDataSave.Robot_x2 = Pose[0].D;
                    PublicData.calibDataSave.Robot_y2 = Pose[1].D;
                    PublicData.calibDataSave.Robot_z2 = Pose[2].D;
                    PublicData.calibDataSave.Robot_g2 = Pose[3].D;
                    PublicData.calibDataSave.Robot_b2 = Pose[4].D;
                    PublicData.calibDataSave.Robot_a2 = Pose[5].D;
                    break;
                case "3":
                    PublicData.calibDataSave.Robot_x3 = Pose[0].D;
                    PublicData.calibDataSave.Robot_y3 = Pose[1].D;
                    PublicData.calibDataSave.Robot_z3 = Pose[2].D;
                    PublicData.calibDataSave.Robot_g3 = Pose[3].D;
                    PublicData.calibDataSave.Robot_b3 = Pose[4].D;
                    PublicData.calibDataSave.Robot_a3 = Pose[5].D;
                    break;
                case "4":
                    PublicData.calibDataSave.Robot_x4 = Pose[0].D;
                    PublicData.calibDataSave.Robot_y4 = Pose[1].D;
                    PublicData.calibDataSave.Robot_z4 = Pose[2].D;
                    PublicData.calibDataSave.Robot_g4 = Pose[3].D;
                    PublicData.calibDataSave.Robot_b4 = Pose[4].D;
                    PublicData.calibDataSave.Robot_a4 = Pose[5].D;
                    break;
                case "5":
                    PublicData.calibDataSave.Robot_x5 = Pose[0].D;
                    PublicData.calibDataSave.Robot_y5 = Pose[1].D;
                    PublicData.calibDataSave.Robot_z5 = Pose[2].D;
                    PublicData.calibDataSave.Robot_g5 = Pose[3].D;
                    PublicData.calibDataSave.Robot_b5 = Pose[4].D;
                    PublicData.calibDataSave.Robot_a5 = Pose[5].D;
                    break;
                case "6":
                    PublicData.calibDataSave.Robot_x6 = Pose[0].D;
                    PublicData.calibDataSave.Robot_y6 = Pose[1].D;
                    PublicData.calibDataSave.Robot_z6 = Pose[2].D;
                    PublicData.calibDataSave.Robot_g6 = Pose[3].D;
                    PublicData.calibDataSave.Robot_b6 = Pose[4].D;
                    PublicData.calibDataSave.Robot_a6 = Pose[5].D;
                    break;
                default:
                    break;
                    #endregion
            }

        }

        void RecMsg(string str)
        {
            //flg = true;
            //msg = str;
        }
        
        
        
        //标定
        private void FButton_Click(object sender, RoutedEventArgs e)
        {
            
            if (!MainWindow.adminflg)
            {
                MessageBox.Show("无修改权限");
                return;
            }
            HTuple CameraHandel;
            HOperatorSet.CreateCalibData("hand_eye_moving_cam", 0, 0, out CameraHandel);

            HTuple[] PicPose = new HTuple[4];
            HTuple[] RobPose = new HTuple[4];

            PicPose[0] = new HTuple(PublicData.calibDataSave.Picture_x1, PublicData.calibDataSave.Picture_y1, PublicData.calibDataSave.Picture_z1, PublicData.calibDataSave.Picture_g1, PublicData.calibDataSave.Picture_b1, PublicData.calibDataSave.Picture_a1, 0);
            PicPose[1] = new HTuple(PublicData.calibDataSave.Picture_x2, PublicData.calibDataSave.Picture_y2, PublicData.calibDataSave.Picture_z2, PublicData.calibDataSave.Picture_g2, PublicData.calibDataSave.Picture_b2, PublicData.calibDataSave.Picture_a2, 0);
            PicPose[2] = new HTuple(PublicData.calibDataSave.Picture_x3, PublicData.calibDataSave.Picture_y3, PublicData.calibDataSave.Picture_z3, PublicData.calibDataSave.Picture_g3, PublicData.calibDataSave.Picture_b3, PublicData.calibDataSave.Picture_a3, 0);
            PicPose[3] = new HTuple(PublicData.calibDataSave.Picture_x4, PublicData.calibDataSave.Picture_y4, PublicData.calibDataSave.Picture_z4, PublicData.calibDataSave.Picture_g4, PublicData.calibDataSave.Picture_b4, PublicData.calibDataSave.Picture_a4, 0);

            RobPose[0] = new HTuple(PublicData.calibDataSave.Robot_x1, PublicData.calibDataSave.Robot_y1, PublicData.calibDataSave.Robot_z1, PublicData.calibDataSave.Robot_g1, PublicData.calibDataSave.Robot_b1, PublicData.calibDataSave.Robot_a1, 0);
            RobPose[1] = new HTuple(PublicData.calibDataSave.Robot_x2, PublicData.calibDataSave.Robot_y2, PublicData.calibDataSave.Robot_z2, PublicData.calibDataSave.Robot_g2, PublicData.calibDataSave.Robot_b2, PublicData.calibDataSave.Robot_a2, 0);
            RobPose[2] = new HTuple(PublicData.calibDataSave.Robot_x3, PublicData.calibDataSave.Robot_y3, PublicData.calibDataSave.Robot_z3, PublicData.calibDataSave.Robot_g3, PublicData.calibDataSave.Robot_b3, PublicData.calibDataSave.Robot_a3, 0);
            RobPose[3] = new HTuple(PublicData.calibDataSave.Robot_x4, PublicData.calibDataSave.Robot_y4, PublicData.calibDataSave.Robot_z4, PublicData.calibDataSave.Robot_g4, PublicData.calibDataSave.Robot_b4, PublicData.calibDataSave.Robot_a4, 0);

            for (int i = 0; i < 4; i++)
            {
                HOperatorSet.SetCalibData(CameraHandel, "tool", i, "tool_in_base_pose", RobPose[i]);
                HOperatorSet.SetCalibDataObservPose(CameraHandel, 0, 0, i, PicPose[i]);
            }
            HTuple Errors=new HTuple();
            try
            {
                HOperatorSet.CalibrateHandEye(CameraHandel, out Errors);
            }
            catch 
            {
                label.Content = "数据采集错误";
            }
            
            if (Errors.Length==4)//显示标定结果
            {
                string inf = (1000*Errors[0].D).ToString("f2")+"_"+ (1000 * Errors[1].D).ToString("f2") +
                    "_" + (1000 * Errors[2].D).ToString("f2") + "_" + (1000 * Errors[3].D).ToString("f2");
                label.Content = inf;


                HTuple ToolTrans;
                HOperatorSet.GetCalibData(CameraHandel, "camera", 0, "tool_in_cam_pose", out ToolTrans);
                HOperatorSet.WriteTuple(ToolTrans,"handeye");
            }
           
            
        }
    }
}
